/*!
  @file   ObjectIdentifier.cpp
  @brief  Identify object inside marker
  @author Kyohei Otsu

  subscribe squared inner image. detect color and shape of the object
*/

#include <cassert>
#include <cmath>
#include <vector>
#include <string>

#include <ros/ros.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>

#include "camera/MarkerImage.h"
#include "camera/MarkerMetaData.h"

#include "ObjectIdentifier.h"


using namespace std;
using namespace cv;
using namespace camera;

namespace enc = sensor_msgs::image_encodings;

namespace marker_detector
{

void ObjectIdentifier::onInit()
{
  ros::NodeHandle& nh = getNodeHandle();
  //ros::NodeHandle& private_nh = getPrivateNodeHandle();
  //image_transport::ImageTransport it(nh);

  // Read params

  // init SVM
  training_from_pngdata();

  // pub/sub
  marker_sub_      = nh.subscribe<MarkerImage>("marker_img", 5, &ObjectIdentifier::imageCallback, this);
  marker_meta_pub_ = nh.advertise<MarkerMetaData>("marker", 2);

}

void ObjectIdentifier::imageCallback( 
    const camera::MarkerImageConstPtr &msg )
{
  cv_bridge::CvImageConstPtr cv_ptr;
  const sensor_msgs::Image img(msg->marker);
  cv_ptr = cv_bridge::toCvCopy(img, enc::BGR8);

  MarkerMetaDataPtr meta(new MarkerMetaData());
  meta->header = msg->header;
  meta->pos    = msg->pos;
  int detected = identify_object(const_cast<cv::Mat&>(cv_ptr->image), meta);

  // should we publish even if the identification fails??
  if(detected>=0) marker_meta_pub_.publish(meta);
}

int ObjectIdentifier::identify_object(cv::Mat &marker, camera::MarkerMetaDataPtr &meta)
{
  // color label
  char color_label[4][8] = { "K", "B", "G", "M" };

  // color extraction
  Mat marker_mono(marker.size(), CV_8UC1);
  int color = extract_color(marker, marker_mono);
  if(color < 0)
  {
    ROS_ERROR("color detection failed");
    return -1;
  }
  imwrite("mono.png", marker_mono);
#if 0
  imshow("marker", marker);
  imshow("mono", marker_mono);
  waitKey(1);
#endif
  meta->color = string(color_label[color]);

  // svm prediction
  Mat _sample_mat(1, MARKER_SIZE*MARKER_SIZE, CV_8UC1, marker_mono.data);
  Mat sample_mat(1, MARKER_SIZE*MARKER_SIZE, CV_32FC1);
  _sample_mat.convertTo(sample_mat, sample_mat.type());
  meta->id = (int) svm_.predict(sample_mat);
  meta->object = label_[meta->id];

  ROS_ERROR("This is %s (%s)!!", label_[meta->id].c_str(), color_label[color]);

  return meta->id;
}

// compute distance between features
float ObjectIdentifier::_get_dist(Feature_t &f1, Feature_t &f2)
{
  float dist = 0.0;
  assert(f1.size() == f2.size());
  for(int i = 0; i < (int)f1.size(); i++) {
    dist += pow(f1[i]-f2[i], 2);
  }
  ROS_INFO("Distance: %f", dist);
  return dist;
}

} // end of namespace

#if 0
int main(int argc, char **argv)
{
  ros::init(argc, argv, "object_identifier");
  object_identifier::ObjectIdentifier oi;
  
  ros::spin();
  return 0;
}
#endif

#include <pluginlib/class_list_macros.h>
PLUGINLIB_DECLARE_CLASS(camera, ObjectIdentifier, marker_detector::ObjectIdentifier, nodelet::Nodelet)
